#include <huniplacer_3d/IoHandlers.h>
#include <huniplacer_3d/huniplacer_3d.h>

#include <GL/freeglut.h>

#include <sstream>

#include <huniplacer_3d/Render.h>
#include <huniplacer_3d/main.h>

#include <huniplacer_3d/CrateContent.h>

KeyData keys;

KeyData* getKeyData(){
	return &keys;
}

void keyHandler(unsigned char key, int x, int y) {
	switch (key) {
	case 'w':
		keys.key_w = true;
		break;
	case 's':
		keys.key_s = true;
		break;
	case 'a':
		keys.key_a = true;
		break;
	case 'd':
		keys.key_d = true;
		break;
	case 'q':
		keys.key_q = true;
		break;
	case 'e':
		keys.key_e = true;
		break;
	case '+':
		keys.key_plus = true;
		break;
	case '-':
		keys.key_min = true;
		break;
	case '\r':
		getHuniplacerData()->modeldata.robot->move_actual_robot();
		break;
	}
}

void keyUpHandler(unsigned char key, int x, int y) {
	switch (key) {
	case 'w':
		keys.key_w = false;
		break;
	case 's':
		keys.key_s = false;
		break;
	case 'a':
		keys.key_a = false;
		break;
	case 'd':
		keys.key_d = false;
		break;
	case 'q':
		keys.key_q = false;
		break;
	case 'e':
		keys.key_e = false;
		break;
	case '+':
		keys.key_plus = false;
		break;
	case '-':
		keys.key_min = false;
		break;
	}
}

void specialHandler(int key, int x, int y) {
	switch (key) {
	case GLUT_KEY_LEFT:
		keys.key_left = true;
		break;
	case GLUT_KEY_RIGHT:
		keys.key_right = true;
		break;
	case GLUT_KEY_UP:
		keys.key_up = true;
		break;
void sendManualDemo(std::string crate_1, int box_1, std::string crate_2, int box_2);
	case GLUT_KEY_DOWN:
		keys.key_down = true;
		break;
	case GLUT_KEY_END:
		exit(1);
		break;
	case GLUT_KEY_F11:
		glutFullScreenToggle();
		break;
	}
}

void sendManualDemo(std::string crate_1, int box_1, std::string crate_2, int box_2);
void specialUpHandler(int key, int x, int y) {
	switch (key) {
	case GLUT_KEY_LEFT:
		keys.key_left = false;
		break;
	case GLUT_KEY_RIGHT:
		keys.key_right = false;
		break;
	case GLUT_KEY_UP:
		keys.key_up = false;
		break;
	case GLUT_KEY_DOWN:
		keys.key_down = false;
		break;
	}
}


/**
 * This function is called when a button in the context menu is pressed. This function will call demo's or switch GUI parts on/off
 * @param item: an int that is bound to a specific menu item
*/
void showMenuItem (int item)
{
	//std::cout << "option " << item << " pressed" << std::endl;
	switch(item){
	case 1: 
		//send ross msg to crate demo			
		sendDemo(1);
		std::cout << "manual demo started" << std::endl;	break;
	case 2:
		sendDemo(2);
		std::cout << "direct copy demo started" << std::endl; break;
	case 3:
		sendDemo(3);
		std::cout << "copy with one spare hole demo started" << std::endl; break;
	case 4:
		sendDemo(4);
		std::cout << "complete mirrored copy demo started" << std::endl; break;	
	case 5:
		sendDemo(5);
		std::cout << "horizontal mirrored copy demo started" << std::endl; break;		
	case 6:
		sendDemo(6);
		std::cout << "vertical mirrored copy demo started" << std::endl; break;	
	case 7:
		sendDemo(7);
		std::cout << "backward diagonal mirror demo started" << std::endl; break;	
	case 8:
		sendDemo(8);
		std::cout << "forward vertical mirror copy demo started" << std::endl; break;	
	case 9:
		sendDemo(9);
		std::cout << "flexible demo started" << std::endl; break;	
	case 10:
		sendDemo(10);
		std::cout << "color demo started" << std::endl; break;	
	case 11:
		sendDemo(11);
		std::cout << " " << std::endl; break;	
	case 111:
		if(getHuniplacerData()->modeldata.draw_motor_0){
			getHuniplacerData()->modeldata.draw_motor_0 = false;
		} else {
			getHuniplacerData()->modeldata.draw_motor_0 = true;
		}
		std::cout << "visibility motor 0 toggled to " << getHuniplacerData()->modeldata.draw_motor_0 << std::endl; break;
	case 112:
		if(getHuniplacerData()->modeldata.draw_motor_1){
			getHuniplacerData()->modeldata.draw_motor_1 = false;
		} else {
			getHuniplacerData()->modeldata.draw_motor_1 = true;
		}
		std::cout << "visibility motor 1 toggled to " << getHuniplacerData()->modeldata.draw_motor_1 << std::endl; break;
	case 113:
		if(getHuniplacerData()->modeldata.draw_motor_2){
			getHuniplacerData()->modeldata.draw_motor_2 = false;
		} else {
			getHuniplacerData()->modeldata.draw_motor_2 = true;
		}
		std::cout << "visibility motor 2 toggled to " << getHuniplacerData()->modeldata.draw_motor_2 << std::endl; break;
	case 120:
		if(getHuniplacerData()->modeldata.draw_glass_plate){
			getHuniplacerData()->modeldata.draw_glass_plate = false;
		} else{
			getHuniplacerData()->modeldata.draw_glass_plate = true;
		}
		std::cout << "visibility glass plate toggled to " << getHuniplacerData()->modeldata.draw_glass_plate << std::endl; break;
	case 130:
		if(getHuniplacerData()->modeldata.draw_crates){
			getHuniplacerData()->modeldata.draw_crates = false;
		} else {
			getHuniplacerData()->modeldata.draw_crates = true;
		}
		std::cout << "visibility crates toggled to " << getHuniplacerData()->modeldata.draw_crates << std::endl; break;
	case 140:
		if(getHuniplacerData()->modeldata.draw_beads){
			getHuniplacerData()->modeldata.draw_beads = false;
		} else {
			getHuniplacerData()->modeldata.draw_beads = true;
		}
		std::cout << "visibility beads toggled to " << getHuniplacerData()->modeldata.draw_beads << std::endl; break;
	}
}

CrateContent* content_selected;

void mouseHandler(int button, int state, int x, int y) {
	//If left mouse button is released
	if (button == 0 && state == 1) {
		picking_render(getHuniplacerData()->modeldata.draw_crates, 
							getHuniplacerData()->modeldata.draw_motor_0, 
							getHuniplacerData()->modeldata.draw_motor_1, 
							getHuniplacerData()->modeldata.draw_motor_2,
							getHuniplacerData()->modeldata.draw_glass_plate, 
							getHuniplacerData()->modeldata.draw_beads, 
							getHuniplacerData()->modeldata.draw_info_panel);
	
		unsigned char pixel_color[3];
		glReadPixels( x, glutGet( GLUT_WINDOW_HEIGHT ) - y, 1, 1, GL_RGB, GL_UNSIGNED_BYTE, pixel_color );
		std::cout << "pixel: " << (int)pixel_color[0] << ":" << (int)pixel_color[1] << ":" << (int)pixel_color[2] << std::endl;

		for( unsigned int i = 0; i < getHuniplacerData()->modeldata.picking_ids.size(); i++ ) {
			if( getHuniplacerData()->modeldata.picking_ids[i]->compare_id( pixel_color ) ) {
				getHuniplacerData()->paneldata.log->write("This is <" + getHuniplacerData()->modeldata.picking_ids[i]->name + ">");
				
				CrateContent* derived_pointer = dynamic_cast<CrateContent*>(getHuniplacerData()->modeldata.picking_ids[i]);
				if (derived_pointer != 0) { //Then this picking_id is of class type CrateContent
					if (content_selected == 0) {
						content_selected = derived_pointer;
						
						content_selected->change_color(0.1, 0.2, 0.8);
					} else {
						//If those are both the same then that means the user has pressed on the same crate at the same index as before.
						//Then the only action must be to deselect the selected, without starting the manual demo.
						if (content_selected->crate->name != derived_pointer->crate->name || 
							 content_selected->index != derived_pointer->index) {
							sendManualDemo(content_selected->crate->name, content_selected->index, derived_pointer->crate->name, derived_pointer->index);
							
							std::ostringstream logData;
							logData 	<< "Manual demo sent. From crate: "  
										<<	content_selected->crate->name 
										<< " with index: " 
										<< content_selected->index
										<< ", to crate: "
										<< derived_pointer->crate->name
										<< " with index: "
										<< derived_pointer->index;
							
							getHuniplacerData()->paneldata.log->write(logData.str());
						} else {
							getHuniplacerData()->paneldata.log->write("Content deselected, no manual demo sent");
						}
						
						content_selected->change_color(1.0, 0.0, 0.0);
						content_selected = 0;
					}
				}
				
				break;
			}
		}
	}
}
